#include <fstream>
#include <string>
#include <iostream>
#include <utils/UtilTimer.h>
#include <services/StateService/StateManagerService.h>

class LogPlayer {
 public:
  void init(char *filename, bool synchrop){
    this->open(filename);
    synchrop_ = synchrop;
    m_first_ = true;
  }

  void open(char *filename){
    ifs.open(filename);
  }

  void close(){
    ifs.close();
  }

  bool read(StateManagerService *man){
    std::string log;
    //ifs >> log;
    if(getline(ifs,log)){
      std::vector<std::string> states = coil::split(log, ",");
      for(unsigned int i=0; i<states.size(); i++){
	std::vector<std::string> elements = coil::split(states[i], " ");
        
	if(i==0){ //abstime
	  if(m_first_){
	    utimer_.start();
	    log_start_time_ = (atol(elements[1].c_str())%100000)*1000 + atol(elements[2].c_str());
	    m_first_ = false;
	  }
	  else{
	    log_current_time_ = (atol(elements[1].c_str())%100000)*1000 + atol(elements[2].c_str()) - log_start_time_;
	    utimer_.tick();
	    slp_time_ = log_current_time_ - static_cast<long int>(utimer_.time()*1000);
	    if(synchrop_ && slp_time_ > 0){
	      usleep(static_cast<unsigned int>(slp_time_ * 1000));
	    }}}

	else{ // rtmstates
	  int data_len = atoi(elements[2].c_str());
	  char sname[256];
	  strcpy(sname, elements[0].c_str());
	  float* m_tmp = (float *)man->getState(sname)->getPtr(0);
	  for(int j=0; j<data_len; j++){
	    m_tmp[j] = atof(elements[j+3].c_str());
	  }}}
      return true;
    }
    return false;
  }

#ifdef DEBUG
  bool read_test(){
    std::string log;
    //ifs >> log;
    if(getline(ifs,log)){
      std::cout << log << std::endl;
      std::vector<std::string> states = coil::split(log, ",");
      for(unsigned int i=0; i<states.size(); i++){
  	std::vector<std::string> elements = coil::split(states[i], " ");
  	std::cout << "state name = " << elements[0] << std::endl;
  	std::cout << "state type = " << elements[1] << std::endl;
  	int data_len = atoi(elements[2].c_str());
  	std::cout << "state num  ="  << data_len << std::endl;
  	for(int i=0; i<data_len; i++){
  	  std::cout << "val" << i << "=" << elements[i+3] << " ";
  	}
  	std::cout << std::endl;
      }
      return true;
    }
    return false;
  }
#endif


 private:
  std::ifstream ifs;
  bool synchrop_;

  long int log_start_time_;
  long int log_current_time_;

  UtilTimer utimer_;
  int slp_time_;

  bool m_first_;

};
